Scalable Graph SLAM for HD Maps

ABSTRACT

A method of solving a graph simultaneous localization and mapping (graph SLAM) for HD maps using a computing system includes partitioning a graph into a plurality of subgraphs, each of the subgraphs having all of the vertices of the graph and a subset of the edges of the graph. Constrained and non-constrained vertices are defined for the subgraphs. An alternating direction method of multipliers (ADMM) formulation for Graph SLAM is defined using the partitioned graph. A distributed Graph SLAM algorithm is then defined in terms of the constrained and non-constrained vertices based on the ADMM formulation. The distributed Graph SLAM algorithm is then used to solve the Graph SLAM problem for HD maps.

CROSS-REFERENCE TO RELATED APPLICATIONS

This application claims priority to U.S. Provisional Application Ser. No. 62/634,327 entitled “SCALABLE GRAPH SLAM FOR HD MAPS” by Irion et al., filed Feb. 23, 2018, the disclosure of which is hereby incorporated herein by reference in its entirety.

TECHNICAL FIELD

The present disclosure is directed to systems and methods for solving Graph SLAM problems, and, in particular, to systems and methods for solving Graph SLAM problems for HD maps.

BACKGROUND

The aim of Graph SLAM is to find the set of poses {x₁, x₂, . . . , x_(N)} that minimizes the cost functional

x ²(x)=Σ_(eij∈ε)(e _(ij)(x))_(t)Ω_(ij) e _(ij)(x),   (1)

where x=(x₁ ^(T), x₂ ^(T), . . . , x_(N) ^(T)). This is typically solved in an iterative manner whereby each iteration k entails approximating the cost functional, finding the optimal set of pose updates Δx^(k) , and computing the next set of poses via

x ^(k+1) =x ^(k)

Δx ^(k),   (2)

where

denotes a pose composition operator. Let d denote the dimensionality of the pose updates (i.e., Δx^(k)∈

^(dN)). Substituting (2) into (1), we compute the gradient b^(k)∈

^(dN) and the Hessian matrix H^(k)∈

^(dNxdN) of the x² error as

$\begin{matrix} {{{x^{2}\left( x^{k + 1} \right)} = {\sum_{e_{ij} \in ɛ}{\left( {e_{ij}\left( {x^{k}\ \mspace{11mu} \Delta \; x^{k}} \right)} \right)^{T}\Omega_{ij}{e_{ij}\left( {x^{k}\ \ \Delta x^{k}} \right)}}}},} & (3) \\ {b^{k} = {2{\sum_{e_{ij} \in ɛ}{\left( {e_{ij}\left( x^{k} \right)} \right)^{T}\Omega_{ij}\frac{\partial e_{ij}}{{\partial\Delta}x^{k}}}}}} & (4) \\ {H^{k} = {2{\sum_{e_{ij} \in ɛ}{\left( \frac{\partial e_{ij}}{{\partial\Delta}x^{k}} \right)^{T}\Omega_{ij}{\frac{\partial e_{ij}}{{\partial\Delta}x^{k}}.}}}}} & (5) \end{matrix}$

The updates x^(k) are obtained by linearizing the edges as

$\begin{matrix} {{e_{ij}\left( x^{k + 1} \right)} \approx {{e_{ij}\left( x^{k} \right)} + {\frac{\partial e_{ij}}{{\partial\Delta}x^{k}}\Delta x^{k}}}} & (6) \end{matrix}$

and substituting into (3), yielding the quadratic approximation

x ²(x ^(k+1))≈½(Δx ^(k))^(T) H ^(k) Δx ^(k)+(b ^(k))^(T) Δx ^(k) +x ²(x ^(k))   (7)

This is minimized by

Δx ^(k)=−(H ^(k))⁻¹ b ^(k)   (8)

Most previously known solvers solve this linear system iteratively on a single node. However, such single-node solvers do not scale well with the size of the mapping region. This necessitates the need for scalable systems and algorithms to build HD Maps for larger areas.

SUMMARY

In accordance with one embodiment of the present disclosure, a method of solving a graph simultaneous localization and mapping (graph SLAM) for HD maps using a computing system includes partitioning a graph into a plurality of subgraphs, each of the subgraphs having all of the vertices of the graph and a subset of the edges of the graph. Constrained and non-constrained vertices are defined for the subgraphs. An alternating direction method of multipliers (ADMM) formulation for Graph SLAM is defined using the partitioned graph. A distributed Graph SLAM algorithm is then defined in terms of the constrained and non-constrained vertices based on the ADMM formulation. The distributed Graph SLAM algorithm is then used to solve the Graph SLAM problem for HD maps.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1(a) shows how the vertices of subgraphs G₁ and G₂ of graph G_(S) are defined;

FIG. 1(b) shows the constrained vertices of subgraphs G₁ and G₂.

FIG. 2 is a schematic depiction of a computing system for implementing the distributed Graph SLAM algorithm for HD maps described herein.

DETAILED DESCRIPTION

For the purposes of promoting an understanding of the principles of the disclosure, reference will now be made to the embodiments illustrated in the drawings and described in the following written specification. It is understood that no limitation to the scope of the disclosure is thereby intended. It is further understood that the present disclosure includes any alterations and modifications to the illustrated embodiments and includes further applications of the principles of the disclosure as would normally occur to a person of ordinary skill in the art to which this disclosure pertains.

There are a variety of frameworks available for large-scale computation and storage. Such frameworks can be broadly categorized as,

-   -   a. Single-node in-memory systems, where the entire data is         loaded and processed in the memory of a single computer.     -   b. In-disk systems, where the entire data resides in disk, and         chunks of it are loaded and processed in memory. Although there         are not many available tools that adopts this approach for         solving (1), most of the single-node solutions can be extended         through the standard in-disk capabilities available in standard         computation platforms. For example, memmapfile in MATLAB, mmap         in Python, etc.     -   c. In-database systems, where the data is stored in a database         and the processing is taken to the database where the data         resides.     -   d. Distributed Storage and Computing systems, where the data         resides in multiple nodes, and the computation is distributed         among those nodes. Typical frameworks involve, Hadoop, Apache         Spark (distributed in-memory analytics with storage on HDFS).

In one embodiment, the Apache Spark computing framework with the data stored in HDFS is used to implement the scalable Graph SLAM system. This offers several advantages. Firstly, compared to the existing single-node based solutions, the distributed architecture allows to scale-out the computation for large scale mapping. Further, compared to other distributed solutions like Hadoop; Spark provides a distributed in-memory computation with lazy execution. This makes it very fast and fault-tolerant, especially for iterative tasks compared to Hadoop(Map-Reduce) which needs to write all intermediate data to the disk. In alternative embodiments, any suitable computing framework may be used.

Utilizing such distributed architectures necessitates the adoption of advanced optimization algorithms that can handle the problem (1) at scale. Basically, the problem in (1) entails solving a quadratic program (QP) iteratively. Recent research towards solving such QPs in a distributed setting typically adopts one of the following approaches:

-   -   a. First Order methods, which use first order information from         the objective like., gradient estimates, secant approximates         etc. Such methods incur low per-iteration computation complexity         and provide dimension independent convergence. However, the         convergence rates heavily depend on the step-size selection and         the conditionality of the problem.     -   b. Second Order methods, which use additional curvature         information like, Hessian (or approximations like L-BFGS), and         improves the conditionality of the problem. Such methods provide         improved convergence rates but results to worse per-step         computation costs.     -   c. Randomized algorithms, which use a subsampled or low rank         representation of the original large scale data to solve a small         scale equivalent problem.

Another broader class of algorithms adopt a splitting technique, which decouples the objective function into smaller sub-problems and solves them iteratively. The resulting sub-problems are easier to tackle and can be readily solved in a distributed fashion using the approaches discussed above. Some such popular algorithms are, Proximal Gradient methods, Proximal (quasi-) Newton methods, Alternating Direction Method of Multipliers (ADMM), etc. Of these approaches ADMM has been widely adopted to solve several large scale practical problems. There are several adaptations of the ADMM algorithm customized to utilize the structure of the problem. One such version well-suited for parallel and distributed settings is consensus ADMM.

Consensus ADMM in it's basic form solves the following problem,

$\begin{matrix} {{\min\limits_{w_{1},\ldots \;,{w_{S,}z}}\Sigma_{s = 1}^{S}{f_{s}\left( w_{s} \right)}}} & (9) \end{matrix}$

subject to w_(S)=z ∀s=1 . . . S.

The consensus ADMM steps are

$\begin{matrix} {{w_{S}^{l + 1} = {{\underset{w_{s}}{argmin}\mspace{11mu} {f_{s}\left( w_{s} \right)}} + {\frac{\rho}{2}{{w_{s} - z^{l} + u_{s}^{l}}}_{2}^{2}}}}\begin{matrix} {z^{l + 1} = {{\underset{z}{argmin}{I_{x_{1}^{l + 1},\; \ldots \;,x_{S}^{l + 1}}(z)}} + {\frac{\rho}{2}{{w_{s}^{l + 1} - z^{l} + u_{s}^{l}}}_{2}^{2}}}} \\ {= {\frac{1}{s}{\sum_{s = 1}^{S}w_{s}^{l + 1}}}} \end{matrix}{u_{s}^{l + 1} = {u_{s}^{l} + w_{s}^{l + 1} - {z^{l + 1}.}}}} & (10) \end{matrix}$

Here, I_(w) ₁ _(, . . . ,w) _(S) (z)={z|w₁= . . . =w_(S)=z}.

Note that, such a form is well-suited for distributed settings using the map-reduce paradigm in Spark. In this case, the w-step involves distributing (mapping) the computation over several nodes ∀s=1 . . . S. And the (consensus) z-step involves aggregating (reducing) the distributed computations at the master node. Further, for (1), the w_(S)-update entails a closed form solution, and can be efficiently computed using standard linear solvers. Although (10) is guaranteed to converge, the convergence rate of the algorithm heavily depends on the conditionality of the problem (1), and the selection of the parameter ρ.

In order to utilize consensus ADMM for Graph SLAM, the graph G=(ν, ε) into a set of subgraphs {G₁, G₂, . . . , G_(S)} where each subgraph includes a number of vertices ν such that ν₁=ν₂= . . . =ν_(S)=ν and the sets of edges ε satisfy ∪_(S=1) ^(S)ε_(S)=εand ε_(S)∩ε_(t)=Ø if s≠t. Define

$\begin{matrix} {{{x_{s}^{2}\left( x_{s}^{k + 1} \right)}:={\sum_{e_{ij} \in ɛ_{s}}{\left( {e_{ij}\left( {x^{k}\ \mspace{11mu} \Delta \; x_{s}^{k}} \right)} \right)^{T}\Omega_{ij}{e_{ij}\left( {x_{s}^{k}\ \ {\Delta x}_{s}^{k}} \right)}}}},} & (11) \\ {{b_{s}^{k}:={{2{\sum_{e_{ij} \in ɛ}{\left( {e_{ij}\left( x^{k} \right)} \right)^{T}\Omega_{ij}\frac{\partial e_{ij}}{{\partial\Delta}x^{k}}}}} \in {\mathbb{R}}^{dN}}},} & (12) \\ {H_{s}^{k}:={{2{\sum_{e_{ij} \in ɛ}{\left( \frac{\partial e_{ij}}{{\partial\Delta}x^{k}} \right)^{T}\Omega_{ij}\frac{\partial e_{ij}}{{\partial\Delta}x^{k}}}}} \in {{\mathbb{R}}^{dN}.}}} & (13) \end{matrix}$

Observe that the gradient and the Hessian matrix, respectively, are given by

$\begin{matrix} { {b^{k} = {\sum_{s = 1}^{S}{b_{s}^{k}.}}}} & (14) \\ { {{H^{k} = {\sum_{s = 1}^{S}H_{s}^{k}}},}} & (15) \\ {\mspace{85mu} {{{{If}\mspace{14mu} \Delta \; x_{1}^{k}} = {{\Delta \; x_{2}^{k}} = {\ldots = {\Delta \; x_{S}^{k}}}}},{{{then}\mspace{11mu} {x^{2}\left( x^{k + 1} \right)}} = {\sum_{s = 1}^{S}{x_{s}^{2}\left( x_{s}^{k + 1} \right)}}},}} & (16) \\ {\approx {{\frac{1}{2}{\sum_{s = 1}^{S}{\left( {\Delta \; x_{S}^{k}} \right)^{T}H_{S}^{k}}}} + {\sum_{s = 1}^{S}{\left( b_{S}^{k} \right)^{T}\Delta \; x_{S}^{k}}} + {\sum_{e_{ij} \in ɛ}{\left( {e_{ij}\left( x^{k} \right)} \right)^{T}\Omega_{ij}{{e_{ij}\left( {x^{k}x^{k}} \right)}.}}}}} & (17) \end{matrix}$

Under the condition that Δx₁ ^(k)=Δx₂ ^(k)= . . . =Δx_(S) ^(k), observe that (17) is equal to (7). Thus, our consensus ADMM formulation for Graph SLAM is

$\begin{matrix} {{{\underset{{{\Delta \; x_{1}^{k}},\ldots \;,\Delta}\;}{\arg \min}\mspace{11mu} \Sigma_{s = 1}^{s}\frac{1}{2}\left( {\Delta x_{s}^{k}} \right)^{T}H_{s}^{k}\Delta x_{s}^{k}} + {\left( b_{s}^{k} \right)^{T}\Delta x_{s}^{k}}},} & (18) \end{matrix}$

subject to Δx₁ ^(k)=Δx₂ ^(k)= . . . =Δ_(N) ^(k), and the resulting over-relaxed ADMM updates are

Δx _(S) ^(k,l+1)=−(H _(S) ^(k) +ρI)⁻¹(b _(S) ^(k) +ρu _(S) ^(k,l) −ρΔx ^(k,l)),   (19)

z ^(k,l+1) =αΔx ^(k,l)+(1−α)z ^(k,l),   (20)

u _(S) ^(k,l+1) =u _(S) ^(k,l) +αΔx ^(k,l+1)+(1−α)z ^(k,l) −z ^(k,l+1)   (21)

Henceforth, the superscript k will be omitted on the ADMM variables Δx, z, and u, and it should be understood that the ADMM iterations are performed as part of the k^(th) Graph SLAM iteration.

While each subgraph G s contains all N vertices (i.e., ν_(S)=ν), it contains only a subset of the edges (i.e., ε_(S)

ε). With that in mind, we define three different classes of vertices (see FIG. 1a for an illustration).

-   -   1) A constrained vertex of subgraph G_(S) is a vertex which is         constrained by at least one edge e∈ε_(S). Let N_(S) denote the         number of constrained vertices in subgraph G_(S). The entries in         Δx_(S) ^(l) and us that correspond to constrained vertices are         denoted as Δx̆_(S) ^(l)∈         ^(dN) ^(S) and ŭ_(S) ^(l)∈         ^(dN) ^(S) , respectively, and the corresponding entries in         b_(S) ^(k) and rows/columns in H_(S) ^(k) are denoted by b̆_(S)         ^(k)∈         ^(dN) ^(S) and H̆_(S) ^(k)∈         ^(dN) ^(S) ^(xdN) ^(S) . Furthermore, let D̆_(S) ^(k)∈         ^(dN) ^(S) ^(xdN) denote the matrix which down-samples to the         entries constrained in subgraph G_(S) (i.e., D̆_(S)Δx̆_(S)         ^(l)=Δx̆_(S) ^(l)∈         ^(dN) ^(S) and D̆_(S) ^(T)Δx̆_(S) ^(l)∈         ^(dN)).     -   2) A non-constrained vertex of subgraph G_(S) is a vertex which         is not constrained by any edges in ε_(S). Let {circumflex over         (D)}_(S)∈         ^(d(N−N) ^(S) ^()xdN) denote the matrix which down-samples to         the entries not constrained in subgraph G_(S). Let ν^(l)∈         ^(dN) denote the values assumed by the non-constrained entries         in u₁ ^(l), . . . , u_(S) ^(l); in other words, {circumflex over         (D)}_(S)v^(l)={circumflex over (D)}_(S)u_(S) ^(l)∈         ^(d(N−N) ^(S) ⁾ for all s.     -   3) When partitioning the graph, each vertex is designated to be         a native vertex of exactly one subgraph for which it is a         constrained vertex; when multiple subgraphs contain edges that         constrain a vertex, the choice of its native subgraph must be         decided. ν_(S,native) to denote the set of vertices which are         native to subgraph G_(S).

A simplified example of graph partitioning is depicted in FIG. 1. The graph G is partitioned into subgraphs G₁ and G₂. The graph G includes four columns and four rows. Referring to FIG. 1(b), the first subgraph G₁ contributes to rows and columns 1-3 of the Hessian, and the second subgraph G₂ contributes to rows and columns 3 and 4. The darker shaded regions indicate native, constrained vertices for subgraph G₁, and the lighter shaded areas indicate native, constrained vertices for subgraph G₂. The subgraphs' non-constrained vertices are not labeled.

To simplify notation, let us define v̆_(S) ^(l):=D̆_(S)v^(l)∈

^(dN), and z̆_(S) ^(l):=D̆_(S)z^(l)∈

^(dN), as the restriction to the entries of v^(l) and z^(l), respectively, that are constrained in subgraph G_(S). Let us define a primal residual for subgraph G_(S) as

ϕ_(S) ^(l) :=Δx _(S) ^(l) −z ^(l)∈

^(dN)   (22) and, as before, let

ϕ̆_(S) ^(l) :32 Δx̆ _(S) ^(l) −z̆ _(S) ^(l)=D̆_(S)ϕ_(S) ^(l)∈

^(dN),   (23)

denote the restriction to entries that correspond to constrained vertices.

With these definitions in place, we can now break down the updates in terms of constrained and non-constrained vertices and present our distributed algorithm for solving Graph SLAM via ADMM:

$\begin{matrix} {{\Delta \; {\overset{\Cup}{x}}_{s}^{l + 1}} = {{- \left( {{\overset{\Cup}{H}}_{s}^{k} + {\rho \; I}} \right)^{- 1}}\left( {{\overset{\Cup}{b}}_{s}^{k} + {\rho \; {\overset{\Cup}{u}}_{s}^{l}} - {\rho \; z_{s}^{l}}} \right)}} & (24) \\ {z^{l + 1} = {z^{l} - {\alpha \; v^{1}} + {\frac{\alpha}{S}{\sum_{s = 1}^{S}{{\overset{\Cup}{D}}_{s}^{T}\left( {{\Delta \; {\overset{\Cup}{x}}_{s}^{l + 1}} - {\overset{\Cup}{z}}_{s}^{l} + {\overset{\Cup}{v}}_{s}^{l}} \right)}}}}} & (25) \\ {\psi^{l + 1} = {z^{l + 1} - z^{l}}} & (26) \\ {v^{l + 1} = {{\left( {1 - \alpha} \right)v^{l}} - \psi^{l + 1}}} & (27) \\ {{\overset{\Cup}{\varphi}}_{s}^{l + 1} = {{\Delta \; {\overset{\Cup}{x}}_{s}^{l + 1}} - {\overset{\Cup}{z}}_{s}^{l + 1}}} & (28) \\ {{\overset{\Cup}{\psi}}_{s}^{l + 1} = {{\overset{\Cup}{z}}_{s}^{l + 1} - {\overset{\Cup}{z}}_{s}^{l}}} & (29) \\ {{\overset{\Cup}{u}}_{s}^{l + 1} = {{\overset{\Cup}{u}}_{s}^{l} + {\alpha \; {\overset{\Cup}{\varphi}}_{s}^{l + 1}} + {\left( {\alpha - 1} \right){\overset{\Cup}{\psi}}_{s}^{l + 1}}}} & (30) \\ {{{\overset{\Cup}{\varphi}}_{s}^{l + 1}}^{2} = {{S{{\psi^{k + 1} = v^{k}}}^{2}} + {\sum_{s = 1}^{S}{{\overset{\Cup}{\varphi}}_{s}^{k + 1}}^{2}} - {{{{\overset{\Cup}{\psi}}_{s}^{k + 1} + {\overset{\Cup}{v}}_{s}^{l}}}^{2}.}}} & (31) \end{matrix}$

The algorithm presented above will converge . . . To obtain a symmetric, positive-definite matrix E^(k) which can be used to precondition the objective function of (18) as

$\begin{matrix} {{{\sum_{s = 1}^{S}{\frac{1}{2}\left( {\Delta x_{s}^{k}} \right)^{T}H_{s}^{k}\Delta x_{s}^{k}}} + {\left( b_{s}^{k} \right)^{T}\Delta x_{s}^{k}}} = {{{\sum_{s = 1}^{S}{\frac{1}{2}\left( {\Delta x_{s}^{k}} \right)^{T}E^{k}H_{s}^{k}E^{k}\Delta x_{S}^{k}}} + {\left( b_{S}^{k} \right)^{T}E^{k}\Delta x_{S}^{k}}} = {{\sum_{s = 1}^{S}{\frac{1}{2}\left( {\Delta {\overset{˜}{x}}_{s}^{k}} \right)^{T}{\overset{¯}{H}}_{s}^{k}\Delta {\overset{˜}{x}}_{S}^{k}}} + {\left( {\overset{\sim}{b}}_{S}^{k} \right)^{T}\Delta {{\overset{˜}{x}}_{S}^{k}.}}}}} & (32) \end{matrix}$

The matrix E^(k) should be selected such that it can be computed and applied in a distributed manner using the subgraphs. Preferably, the term E_(k)H_(S) ^(k)E^(k)=Σ_(S=1) ^(S)E^(k)H_(S) ^(k)E^(k)≈I. In order to achieve this, the Hessian H_(S) ^(k) is approximated for each subgraph using only contributions to the Hessian in rows and columns that correspond to vertices which are native to the subgraph (see FIG. 1(b). For example, suppose e_(ij) is a binary edge which constrains vertices ν_(i) and ν_(j). Let Δx_(i) ^(k):=1_(i)⊙Δx^(k), where ⊙ denotes element-wise multiplication and 1_(i)∈

^(dN) denotes a vector which is 1 in entries that correspond to vertex ν_(i) and 0 elsewhere, and define Δx_(j) ^(k) accordingly. The edge may be defined as

$\begin{matrix} {{e_{\lbrack{i,j}\rbrack}\left( {x^{k}\Delta \; x^{k}} \right)}:=\left. {{e_{ij}\left( x^{k} \right)} + {\delta_{\underset{v_{j} \notin v_{s,{native}}}{v_{i} \in v_{s,{native}}}}\frac{\partial}{{\partial\Delta}\; x_{i}^{\prime}}{e_{ij}\left( {x^{k}\Delta \; x^{\prime}} \right)}}} \middle| {}_{{\Delta \; x^{\prime}} = 0}{{\Delta \; x^{k}} + {\delta_{\underset{v_{j} \notin v_{s,{native}}}{v_{i} \in v_{s,{native}}}}\frac{\partial}{{\partial\Delta}\; x_{j}^{\prime}}{e_{ij}\left( {x^{k}\Delta \; x^{\prime}} \right)}}} \middle| {}_{{\Delta \; x^{\prime}} = 0}{{\Delta \; x^{k}} + {\delta_{\underset{v_{j} \notin v_{s,{native}}}{v_{i} \in v_{s,{native}}}}\frac{\partial}{{\partial\Delta}\; x^{\prime}}{e_{ij}\left( {x^{k}\Delta \; x^{\prime}} \right)}}} \middle| {}_{{\Delta \; x^{\prime}} = 0}{\Delta \; x^{k}} \right.} & (33) \end{matrix}$

where δ is the Kronecker delta. Note that e_(ij)(x)=e_([ij])(x), so the x² error and optimization problem remain unchanged. The native Hessian H_(S,native) ^(k) for a subgraph is defined as

$\begin{matrix} {{H_{s,{native}}^{k}:} = {2{\sum_{{e{\lbrack{ij}\rbrack}} \in ɛ_{s}}{\left( \frac{\partial e_{\lbrack{ij}\rbrack}}{{\partial\Delta}x^{k}} \right)^{T}\Omega_{ij}{\frac{\partial e_{\lbrack{ij}\rbrack}}{{\partial\Delta}x^{k}}.}}}}} & (34) \end{matrix}$

Note that the supports of H_(1,native) ^(k), . . . , H_(S,native) are disjoint. This allows for fast, distributed computations, and so the preconditioning matrix is defined as

E ^(k):=(Σ_(S=1) ^(S) H _(S,native) ^(k))^(−1/2)=Σ_(S=1) ^(S)(H _(S,native) ^(k))^(−1/2)   (35)

By redefining the edges, as in (33), more constraints can be accounted for which in turn enables the Hessian to be better approximates, yielding a better preconditioning matrix.

It is worth mentioning that a subgraph's Hessian H_(S) ^(k) and its native Hessian H_(S,native) ^(k) can both contain contributions that the other lacks. This is illustrated in FIG. 1, where edge e₂₃∈ε₁ constrains vertices ν₂∈ν_(1,native) and ν₃∈ν_(2,native). The Hessian H₁ ^(k) contains contributions from edge e₂₃ in the (2, 2), (2, 3), (3, 2), and (3, 3) blocks, but e₂₃ only contributes to the (2, 2) block of H_(1,native) ^(k). On the other hand, the native Hessian H_(2,native) ^(k) contains the (3, 3) block contribution from e₂₃ but H₂ ^(k) does not contain any contributions from e₂₃. As vertices ν₂ and ν₃ are native to different subgraphs, the (2, 3) and (3, 2) block contributions from edge e₂₃ do not contribute to any native Hessians—such is the price that must be paid to ensure that the algorithm remains distributed. This also demonstrates the importance of partitioning the graph in such a way that the number of edges that span different subgraphs is minimized.

FIG. 2 depicts an exemplary system 100 for implementing the scalable Graph SLAM framework described above. The system 100 includes a processing system 102 that is operatively connected to a memory system 104. The processing system 102 includes one or more processors which may be located in the same device/location or may be distributed across multiple devices at one or more locations. Any suitable type of processor(s) may be used.

The processing system is operably connected to the memory system 104. The memory system 104 may include any suitable type of non-transitory computer readable storage medium. The memory system 104 may be distributed across multiple devices and/or locations. Programmed instructions are stored in the memory system. The programmed instructions include the instructions for implementing the operating system and various functionality of the system. The programmed instructions also include instructions for implementing the scalable Graph SLAM and ADMM algorithms described herein.

The components of the system may be communicatively connected by one or more networks 106. Any suitable type of network(s) may be used including wired and wireless types of networks. The computing devices include the appropriate network interface devices that enable transmitting and receiving of data via the network(s).

While the disclosure has been illustrated and described in detail in the drawings and foregoing description, the same should be considered as illustrative and not restrictive in character. It is understood that only the preferred embodiments have been presented and that all changes, modifications and further applications that come within the spirit of the disclosure are desired to be protected. 

What is claimed is:
 1. A method of solving a graph simultaneous localization and mapping (graph SLAM) problem for HD maps using a computing system, the method comprising: partitioning a graph into a plurality of subgraphs, the graph including a plurality of vertices and a plurality of edges that extend between vertices, each of the vertices corresponding to a pose during mapping, each of the edges defining a spatial constraint between two vertices, wherein each of the subgraphs includes all of the vertices of the graphs, and wherein each of the subgraphs include a subset of the edges of the graph; defining constrained, non-constrained and native vertices for each of the subgraphs; defining an alternating direction method of multipliers (ADMM) formulation for Graph SLAM based on the partitioned graph; defining updates for the ADMM formulation based on the ADMM algorithm; defining updates for a distributed Graph SLAM algorithm on the updates for the ADMM formulation and in terms of the constrained and the non-constrained vertices of the subgraphs; and solving the updates for the distributed Graph SLAM algorithm to solve the Graph SLAM problem.
 2. The method of claim 1, wherein the ADMM formulation is a consensus ADMM formulation given by ${{\underset{{\Delta \; x_{1}^{k}},\ldots \;,{\Delta \; x_{N}^{k}}}{{\arg \min}\;}{\sum_{s = 1}^{S}{\frac{1}{2}\left( {\Delta x_{s}^{k}} \right)^{T}H_{s}^{k}\Delta x_{s}^{k}}}} + {\left( b_{s}^{k} \right)^{T}\Delta x_{S}^{k}}},{{{subject}\mspace{14mu} {to}\mspace{14mu} \Delta \; x_{1}^{k}} = {{\Delta x_{2}^{k}} = {\ldots = {\Delta \; x_{N}^{k}}}}}$ where k is a number of iterations, s is a number of graph nodes and T is a number of poses.
 3. The method of claim 2, wherein the updates for the consensus ADMM formulation are given by Δx _(S) ^(k,l+1)=−(H _(S) ^(k) +ρI)⁻¹(b _(S) ^(k) +ρu _(S) ^(k,l) −ρΔx ^(k,l)), z ^(k,l+1) =αΔx ^(k,l)+(1−α)z ^(k,l), u _(S) ^(k,l+1) =u _(S) ^(k,l) +αΔx ^(k,l+1)+(1−α)z ^(k,l) −z ^(k,l+1).
 4. The method of claim 3, wherein v̆_(S) ^(l):=D̆_(S)v^(l)∈

^(dN), and z̆_(S) ^(l):=D̆_(S)z^(l)∈

^(dN) define restriction of entries of v^(l) and z^(l), respectively, that are constrained in subgraph G_(S), wherein a primal residual for subgraph G_(S) is defined as ϕ_(S) ^(l):=Δx_(S) ^(l)−z^(l)∈

^(dN), and wherein ϕ̆_(S) ^(l):=Δx̆_(S) ^(l)−z̆_(S) ^(l)=D̆_(S)ϕ_(S) ^(l)∈

^(dN) denotes restriction to entries that correspond to constrained vertices.
 5. The method of claim 4, wherein the updates for the distributed Graph SLAM algorithm are defined in terms of constrained and non-constrained vertices such that $\begin{matrix} {{\Delta \; {\overset{\Cup}{x}}_{s}^{l + 1}} = {{- \left( {{\overset{\Cup}{H}}_{s}^{k} + {\rho \; I}} \right)^{- 1}}\left( {{\overset{\Cup}{b}}_{s}^{k} + {\rho \; {\overset{\Cup}{u}}_{s}^{l}} - {\rho \; z_{s}^{l}}} \right)}} \\ {z^{l + 1} = {z^{l} - {\alpha \; v^{1}} + {\frac{\alpha}{S}{\sum_{s = 1}^{S}{{\overset{\Cup}{D}}_{s}^{T}\left( {{\Delta \; {\overset{\Cup}{x}}_{s}^{l + 1}} - {\overset{\Cup}{z}}_{s}^{l} + {\overset{\Cup}{v}}_{s}^{l}} \right)}}}}} \\ {\psi^{l + 1} = {z^{l + 1} - z^{l}}} \\ {v^{l + 1} = {{\left( {1 - \alpha} \right)v^{l}} - \psi^{l + 1}}} \\ {{\overset{\Cup}{\varphi}}_{s}^{l + 1} = {{\Delta \; {\overset{\Cup}{x}}_{s}^{l + 1}} - {\overset{\Cup}{z}}_{s}^{l + 1}}} \\ {{\overset{\Cup}{\psi}}_{s}^{l + 1} = {{\overset{\Cup}{z}}_{s}^{l + 1} - {\overset{\Cup}{z}}_{s}^{l}}} \\ {{\overset{\Cup}{u}}_{s}^{l + 1} = {{\overset{\Cup}{u}}_{s}^{l} + {\alpha \; {\overset{\Cup}{\varphi}}_{s}^{l + 1}} + {\left( {\alpha - 1} \right){\overset{\Cup}{\psi}}_{s}^{l + 1}}}} \\ {{{\overset{\Cup}{\varphi}}_{s}^{l + 1}}^{2} = {{S{{\psi^{k + 1} = v^{k}}}^{2}} + {\sum_{s = 1}^{S}{{\overset{\Cup}{\varphi}}_{s}^{k + 1}}^{2}} - {{{{\overset{\Cup}{\psi}}_{s}^{k + 1} + {\overset{\Cup}{v}}_{s}^{l}}}^{2}.}}} \end{matrix}$
 6. The method of claim 1, wherein the computing system comprises a distributed storage and computing system where the data resides in multiple nodes, and the computation is distributed among those nodes.
 7. The method of claim 6, wherein the computing system has an Apache Spark framework with data stored on a Hadoop Distributed File System (HDFS). 